#include "qmi8658c.h"
#include "driver/i2c.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include <math.h>

#define I2C_MASTER_PORT      I2C_NUM_0

static const char *TAG = "QMI8658C";

// 读取指定寄存器
esp_err_t qmi8658c_register_read(uint8_t reg_addr, uint8_t *data_rd, size_t size)
{
    return i2c_master_write_read_device(I2C_MASTER_PORT, QMI8658C_DEV_ADDR, &reg_addr, 1, data_rd, size, pdMS_TO_TICKS(1000));
}

// 向指定寄存器写入一个字节
esp_err_t qmi8658c_register_write_byte(uint8_t reg_addr, uint8_t data)
{
    // 缓冲区用于存储寄存器地址和数据
    uint8_t write_buf[2] = {reg_addr, data};
    return i2c_master_write_to_device(I2C_MASTER_PORT, QMI8658C_DEV_ADDR, write_buf, sizeof(write_buf), pdMS_TO_TICKS(1000));
}

// 初始化 QMI8658C
void qmi8658c_init(void)
{
    uint8_t id = 0;
    // 检查设备 ID，直到识别到 QMI8658C
    while (id != QMI8658C_DEVICE_ID)
    {
        // 读取设备 ID
        qmi8658c_register_read(QMI8658C_WHO_AM_I, &id, 1);
        vTaskDelay(pdMS_TO_TICKS(1000));
    }

    ESP_LOGI(TAG, "QMI8658C initialized!");
    // 进行一系列的寄存器写入配置
    qmi8658c_register_write_byte(QMI8658C_RESET_REG, 0xB0);  // 复位
    vTaskDelay(pdMS_TO_TICKS(10));               // 延时
    qmi8658c_register_write_byte(QMI8658C_CTRL1, 0x40);    // CTRL1 设置地址自动增加
    qmi8658c_register_write_byte(QMI8658C_CTRL7, 0x03);    // CTRL7 允许加速器和陀螺仪
    qmi8658c_register_write_byte(QMI8658C_CTRL2, 0x95);    // CTRL2 设置 ACC 4g 250Hz
    qmi8658c_register_write_byte(QMI8658C_CTRL3, 0xD5);    // CTRL3 设置 GRY 512dps 250Hz
}

// 读取加速度计和陀螺仪的数据
void qmi8658c_Read_AccAndGry(t_sQMI8658C *p)
{
    // 状态和数据准备标志
    uint8_t status, data_ready = 0;
    // 数据缓存
    int16_t buf[6];
    // 读取状态寄存器
    qmi8658c_register_read(QMI8658C_STATUSOU, &status, 1);
    // 检查加速度计和陀螺仪数据是否准备好
    if ((status & QMI8658C_DATA_READY_MASK_ACCEL) && (status & QMI8658C_DATA_READY_MASK_GYRO))
    {
        data_ready = 1;
    }
    if (data_ready)
    {
        // 读取加速度计和陀螺仪数据
        qmi8658c_register_read(QMI8658C_AX_L, (uint8_t *)buf, 12);
        // 将读取的数据转换为坐标值
        p->accel_x = (int16_t)(buf[0] | (buf[1] << 8));
        p->accel_y = (int16_t)(buf[2] | (buf[3] << 8));
        p->accel_z = (int16_t)(buf[4] | (buf[5] << 8));
        p->gyro_x = (int16_t)(buf[6] | (buf[7] << 8));
        p->gyro_y = (int16_t)(buf[8] | (buf[9] << 8));
        p->gyro_z = (int16_t)(buf[10] | (buf[11] << 8));
    }
}

// 获取 QMI8658C 的角度
void qmi8658c_fetch_angleFromAcc(t_sQMI8658C *p)
{
    // 临时变量
    float temp;
    // 读取数据
    qmi8658c_Read_AccAndGry(p);
    // 计算并且转换为角度
    temp = (float)p->accel_x / sqrt(((float)p->accel_y * (float)p->accel_y + (float)p->accel_z * (float)p->accel_z));
    p->angle_x = atan(temp) * 57.3f;
    temp = (float)p->accel_y / sqrt(((float)p->accel_x * (float)p->accel_x + (float)p->accel_z * (float)p->accel_z));
    p->angle_y = atan(temp) * 57.3f;
    temp = (float)p->accel_z / sqrt(((float)p->accel_x * (float)p->accel_x + (float)p->accel_y * (float)p->accel_y));
    p->angle_z = atan(temp) * 57.3f;
}



